/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
#define CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_

#include <utility>
#include <vector>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/mapping/2d/xy_index.h"
#include "cartographer/mapping/proto/map_limits.pb.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer {
	namespace mapping {

		// Defines the limits of a grid map. This class must remain inlined for
		// performance reasons.
		class MapLimits {
		public:
			MapLimits(const double resolution, const Eigen::Vector2d& max,
				const CellLimits& cell_limits)
				: resolution_(resolution), max_(max), cell_limits_(cell_limits) {
				CHECK_GT(resolution_, 0.);
				CHECK_GT(cell_limits.num_x_cells, 0.);
				CHECK_GT(cell_limits.num_y_cells, 0.);
			}

			explicit MapLimits(const proto::MapLimits& map_limits)
				: resolution_(map_limits.resolution()),
				max_(transform::ToEigen(map_limits.max())),
				cell_limits_(map_limits.cell_limits()) {}

			// Returns the cell size in meters. All cells are square and the resolution is
			// the length of one side.
			double resolution() const { return resolution_; }

			// Returns the corner of the limits, i.e., all pixels have positions with
			// smaller coordinates.
			const Eigen::Vector2d& max() const { return max_; }

			// Returns the limits of the grid in number of cells.
			const CellLimits& cell_limits() const { return cell_limits_; }

			// Returns the index of the cell containing the 'point' which may be outside
			// the map, i.e., negative or too large indices that will return false for
			// Contains().
			Eigen::Array2i GetCellIndex(const Eigen::Vector2f& point) const {
				// Index values are row major and the top left has Eigen::Array2i::Zero()
				// and contains (centered_max_x, centered_max_y). We need to flip and
				// rotate.
				return Eigen::Array2i(
					common::RoundToInt((max_.y() - point.y()) / resolution_ - 0.5),
					common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
			}

			// Returns the center of the cell at 'cell_index'.
			Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
				return { max_.x() - resolution() * (cell_index[1] + 0.5),
						max_.y() - resolution() * (cell_index[0] + 0.5) };
			}

			// Returns true if the ProbabilityGrid contains 'cell_index'.
			bool Contains(const Eigen::Array2i& cell_index) const {
				return (Eigen::Array2i(0, 0) <= cell_index).all() &&
					(cell_index <
						Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
					.all();
			}

		private:
			double resolution_;//地图的分辨率，即一个栅格单元对应的地图尺寸
			Eigen::Vector2d max_;//记录了地图的x和y方向上的最大值
			CellLimits cell_limits_;//x和y方向上的栅格数量
		};

		inline proto::MapLimits ToProto(const MapLimits& map_limits) {
			proto::MapLimits result;
			result.set_resolution(map_limits.resolution());
			*result.mutable_max() = transform::ToProto(map_limits.max());
			*result.mutable_cell_limits() = ToProto(map_limits.cell_limits());
			return result;
		}

	}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
